
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_channels.c
  * @author     baiyang
  * @date       2022-11-16
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "rc_channel.h"

#include <string.h>

#include <rtdebug.h>

#include <uITC/uITC_msg.h>
#include <parameter/param.h>
#include <common/time/gp_time.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define RC_PARAM_VAL_INFO(x, min, max, trim, reversed, dz, option) \
    PARAM_DEFINE_INT16(RC##x##_MIN, min), \
    PARAM_DEFINE_INT16(RC##x##_MAX, max), \
    PARAM_DEFINE_INT16(RC##x##_TRIM, trim), \
    PARAM_DEFINE_INT16(RC##x##_REVERSED, reversed), \
    PARAM_DEFINE_INT16(RC##x##_DZ, dz), \
    PARAM_DEFINE_INT16(RC##x##_OPTION, option)

#define RC_PARAM_LINK_VARIABLE(rc, x) \
    param_link_variable(PARAM_ID(RC, RC##x##_MIN), &rc->radio_min); \
    param_link_variable(PARAM_ID(RC, RC##x##_MAX), &rc->radio_max); \
    param_link_variable(PARAM_ID(RC, RC##x##_TRIM), &rc->radio_trim); \
    param_link_variable(PARAM_ID(RC, RC##x##_REVERSED), &rc->reversed); \
    param_link_variable(PARAM_ID(RC, RC##x##_DZ), &rc->dead_zone); \
    param_link_variable(PARAM_ID(RC, RC##x##_OPTION), &rc->option)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void rcs_assign_param();
static void rcs_publish_vehicle_rc();
/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    // @Param: _OVERRIDE_TIME
    // @DisplayName: RC override timeout
    // @Description: Timeout after which RC overrides will no longer be used, and RC input will resume, 0 will disable RC overrides, -1 will never timeout, and continue using overrides until they are disabled
    // @User: Advanced
    // @Range: 0.0 120.0
    // @Units: s
    PARAM_DEFINE_FLOAT(RC_OVERRIDE_TIME, 3.0f),

    // @Param: _OPTIONS
    // @DisplayName: RC options
    // @Description: RC input options
    // @User: Advanced
    // @Bitmask: 0:Ignore RC Receiver, 1:Ignore MAVLink Overrides, 2:Ignore Receiver Failsafe bit but allow other RC failsafes if setup, 3:FPort Pad, 4:Log RC input bytes, 5:Arming check throttle for 0 input, 6:Skip the arming check for neutral Roll/Pitch/Yaw sticks, 7:Allow Switch reverse, 8:Use passthrough for CRSF telemetry, 9:Suppress CRSF mode/rate message for ELRS systems,10:Enable multiple receiver support, 11:CRSF RSSI shows Link Quality
    PARAM_DEFINE_INT32(RC_OPTIONS, RCS_ARMING_CHECK_THROTTLE),

    // @Param: _PROTOCOLS
    // @DisplayName: RC protocols enabled
    // @Description: Bitmask of enabled RC protocols. Allows narrowing the protocol detection to only specific types of RC receivers which can avoid issues with incorrect detection. Set to 1 to enable all protocols.
    // @User: Advanced
    // @Bitmask: 0:All,1:PPM,2:IBUS,3:SBUS,4:SBUS_NI,5:DSM,6:SUMD,7:SRXL,8:SRXL2,9:CRSF,10:ST24,11:FPORT,12:FPORT2,13:FastSBUS
    PARAM_DEFINE_INT32(RC_PROTOCOLS, 1),

    // @Param: _FS_TIMEOUT
    // @DisplayName: RC Failsafe timeout
    // @Description: RC failsafe will trigger this many seconds after loss of RC
    // @User: Standard
    // @Range: 0.5 10.0
    // @Units: s
    PARAM_DEFINE_FLOAT(RC_FS_TIMEOUT, 1.0f),

    RC_PARAM_VAL_INFO(1, 1100, 1900, 1500, 0, 20, 0),
    RC_PARAM_VAL_INFO(2, 1100, 1900, 1500, 0, 20, 0),
    RC_PARAM_VAL_INFO(3, 1100, 1900, 1500, 0, 30, 0),
    RC_PARAM_VAL_INFO(4, 1100, 1900, 1500, 0, 20, 0),
    RC_PARAM_VAL_INFO(5, 1100, 1900, 1500, 0, 20, 0),
    RC_PARAM_VAL_INFO(6, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(7, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(8, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(9, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(10, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(11, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(12, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(13, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(14, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(15, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(16, 1100, 1900, 1500, 0,  0, 0),
#if NUM_RC_CHANNELS > 16
    RC_PARAM_VAL_INFO(17, 1100, 1900, 1500, 0,  0, 0),
    RC_PARAM_VAL_INFO(18, 1100, 1900, 1500, 0,  0, 0),
#endif
};
PARAM_GROUP_DEFINE(RC, var_info);

struct rcs_channel rcs;

static uitc_actuator_rc actuator_rc;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void rcs_ctor(rcs_ops_t ops)
{
    rcs.ops = ops;

    rcs._gcs_overrides_enabled = true;

    // setup ch_in on channels
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rcs.channels[i] = rcs_get_channel(i);
    }
}

rcs_channel_t rcs_get_singleton()
{
    return &rcs;
}

rc_channel_t rcs_chan(const uint8_t chan)
{
    if (chan >= NUM_RC_CHANNELS) {
        return NULL;
    }

    return rcs.channels[chan];
}

void rcs_init(void)
{
    rcs_assign_param();

    // setup ch_in on channels
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rcs_chan(i)->ch_in = i;
    }

    rcs_init_aux_all();
}

void rcs_init_aux_all()
{
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rc_channel_t c = rcs_chan(i);
        if (c == NULL) {
            continue;
        }
        rc_init_aux(c);
    }

    rcs_reset_mode_switch();
}

// support for auxiliary switches:
// read_aux_switches - checks aux switch positions and invokes configured actions
void rcs_read_aux_all()
{
    if (!rcs_has_valid_input()) {
        // exit immediately when no RC input
        return;
    }

    //bool need_log = false;

    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rc_channel_t c = rcs_chan(i);
        if (c == NULL) {
            continue;
        }

        (void)rc_read_aux(c);

        //need_log |= rc_read_aux(c);
    }

#if 0
    if (need_log) {
        // guarantee that we log when a switch changes
        AP::logger().Write_RCIN();
    }
#endif
}

// compatability functions for Plane:
uint16_t rcs_get_radio_in(const uint8_t chan)
{
    rc_channel_t c = rcs_chan(chan);
    if (c == NULL) {
        return 0;
    }

    return rc_get_radio_in(c);
}

uint8_t rcs_get_radio_in2(uint16_t *chans, const uint8_t num_channels)
{
    memset(chans, 0, num_channels*sizeof(*chans));

    const uint8_t read_channels = MIN(num_channels, NUM_RC_CHANNELS);
    for (uint8_t i = 0; i < read_channels; i++) {
        chans[i] = rc_get_radio_in(rcs_chan(i));
    }

    return read_channels;
}

// update all the input channels
bool rcs_read_input(void)
{
    if (rc_hal_new_input()) {
        rcs._has_had_rc_receiver = true;
    } else if (!rcs.has_new_overrides) {
        return false;
    }

    rcs.has_new_overrides = false;

    rcs.last_update_ms = time_millis();

    bool success = false;
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        success |= rc_update(rcs_chan(i));
    }

    rcs_publish_vehicle_rc();

    return success;
}

uint8_t rcs_get_valid_channel_count(void)
{
    return MIN(NUM_RC_CHANNELS, rc_hal_num_channels());
}

int16_t rcs_get_receiver_rssi(void)
{
    return rc_hal._rssi;
}

int16_t rcs_get_receiver_link_quality(void)
{
    return rc_hal._rx_link_quality;
}

uint16_t rcs_get_override_mask(void)
{
    uint16_t ret = 0;
    for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) {
        if (rc_has_override(rcs_chan(i))) {
            ret |= (1U << i);
        }
    }
    return ret;
}

bool rcs_gcs_overrides_enabled()
{
    return rcs._gcs_overrides_enabled;
}

void rcs_set_gcs_overrides_enabled(bool enable)
{
    rcs._gcs_overrides_enabled = enable;
    if (!rcs._gcs_overrides_enabled) {
        rcs_clear_overrides();
    }
}

void rcs_clear_overrides(void)
{
    for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) {
        rc_clear_override(rcs_chan(i));
    }
    // we really should set has_new_overrides to true, and rerun read_input from
    // the vehicle code however doing so currently breaks the failsafe system on
    // copter and plane, RC_Channels needs to control failsafes to resolve this
}

void rcs_set_override(const uint8_t chan, const int16_t value, const uint32_t timestamp_ms)
{
    if (chan < NUM_RC_CHANNELS) {
        rc_set_override(rcs_chan(chan), value, timestamp_ms);
    }
}

bool rcs_has_active_overrides()
{
    for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) {
        if (rc_has_override(rcs_chan(i))) {
            return true;
        }
    }

    return false;
}

void rcs_new_override_received()
{
    rcs.has_new_overrides = true;
    rcs._has_had_override = true;
}

// returns true if overrides should time out.  If true is returned
// then returned_timeout_ms will contain the timeout in
// milliseconds, with 0 meaning overrides are disabled.
bool rcs_get_override_timeout_ms(uint32_t *returned_timeout_ms)
{
    const float value = rcs._override_timeout;
    if (math_flt_positive(value)) {
        *returned_timeout_ms = (uint32_t)(value * 1e3f);
        return true;
    }
    if (math_flt_zero(value)) {
        *returned_timeout_ms = 0;
        return true;
    }
    // overrides will not time out
    return false;
}

// get failsafe timeout in milliseconds
uint32_t rcs_get_fs_timeout_ms()
{
    return MAX(rcs._fs_timeout * 1000, 100);
}

// should a channel reverse option affect aux switches
bool rcs_switch_reverse_allowed(void)
{
    return (rcs._options & (uint32_t)(RCS_ALLOW_SWITCH_REV));
}

// return switch position value as LOW, MIDDLE, HIGH
// if reading the switch fails then it returns LOW
enum RcAuxSwitchPos rcs_get_channel_pos(const uint8_t rcmapchan)
{
    const struct rc_channel* chan = rcs_chan(rcmapchan-1);
    return chan != NULL ? rc_get_aux_switch_pos(chan) : RC_AUX_SWITCH_LOW;
}

rc_channel_t rcs_find_channel_for_option(const rc_aux_func_t option)
{
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rc_channel_t c = rcs_chan(i);
        if (c == NULL) {
            // odd?
            continue;
        }
        if ((rc_aux_func_t)c->option == option) {
            return c;
        }
    }

    return NULL;
}

// duplicate_options_exist - returns true if any options are duplicated
bool rcs_duplicate_options_exist()
{
    uint8_t auxsw_option_counts[256] = {0};
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rc_channel_const_t c = rcs_chan(i);
        if (c == NULL) {
            // odd?
            continue;
        }
        const uint16_t option = c->option;
        if (option >= sizeof(auxsw_option_counts)) {
            continue;
        }
        auxsw_option_counts[option]++;
    }

    for (uint16_t i=0; i<sizeof(auxsw_option_counts); i++) {
        if (i == 0) { // MAGIC VALUE! This is AUXSW_DO_NOTHING
            continue;
        }
        if (auxsw_option_counts[i] > 1) {
            return true;
        }
    }
   return false;
}

// convert option parameter from old to new
void rcs_convert_options(const rc_aux_func_t old_option, const rc_aux_func_t new_option)
{
    param_t* option = NULL;

    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rc_channel_t c = rcs_chan(i);
        if (c == NULL) {
            // odd?
            continue;
        }
        if ((rc_aux_func_t)c->option == old_option) {
            option = param_get_by_variable2("RC", &c->option);

            if (option != NULL) {
                param_set_and_save(option, (int16_t)new_option);
            } else {
                c->option = (int16_t)new_option;
            }
        }
    }
}

// should we ignore RC failsafe bits from receivers?
bool rcs_ignore_rc_failsafe(void)
{
    return (rcs._options & (uint32_t)(RCS_IGNORE_FAILSAFE));
}

bool rcs_ignore_overrides()
{
    return rcs._options & (uint32_t)(RCS_IGNORE_OVERRIDES);
}

bool rcs_ignore_receiver()
{
    return rcs._options & (uint32_t)(RCS_IGNORE_RECEIVER);
}

bool rcs_arming_skip_checks_rpy() {
    return rcs._options & (uint32_t)(RCS_ARMING_SKIP_CHECK_RPY);
}

bool rcs_suppress_crsf_message(void) {
    return (rcs._options & (uint32_t)(RCS_SUPPRESS_CRSF_MESSAGE));
}

bool rcs_multiple_receiver_support() {
    return rcs._options & (uint32_t)(RCS_MULTI_RECEIVER_SUPPORT);
}

bool rcs_use_crsf_lq_as_rssi(void) {
    return (rcs._options & (uint32_t)(RCS_USE_CRSF_LQ_AS_RSSI)) != 0;
}

// returns true if we have had a direct detach RC reciever, does not include overrides
bool rcs_has_had_rc_receiver()
{
    return rcs._has_had_rc_receiver;
}

// returns true if we have had an override on any channel
bool rcs_has_had_rc_override()
{
    return rcs._has_had_override;
}

//
// Support for mode switches
//
rc_channel_t rcs_flight_mode_channel()
{
    const int8_t num = rcs_flight_mode_channel_number();

    if (num <= 0) {
        return NULL;
    }
    if (num >= NUM_RC_CHANNELS) {
        return NULL;
    }

    return rcs_chan(num-1);
}

void rcs_reset_mode_switch()
{
    rc_channel_t c = rcs_flight_mode_channel();
    if (c == NULL) {
        return;
    }
    rc_reset_mode_switch(c);
}

// check if flight mode channel is assigned RC option
// return true if assigned
bool rcs_flight_mode_channel_conflicts_with_rc_option()
{
    rc_channel_t chan = rcs_flight_mode_channel();
    if (chan == NULL) {
        return false;
    }
    return (rc_aux_func_t)chan->option != RC_AUX_DO_NOTHING;
}

// return mask of enabled protocols.
uint32_t rcs_enabled_protocols() {
    return (uint32_t)(rcs._protocols);
}

uint32_t rcs_last_input_ms() {
    return rcs.last_update_ms;
}

// method for other parts of the system (e.g. Button and mavlink)
// to trigger auxiliary functions
bool rcs_run_aux_function(rc_aux_func_t ch_option, enum RcAuxSwitchPos pos, enum RcAuxFuncTriggerSource source) {
    return rc_run_aux_function(rcs_chan(0), ch_option, pos, source);
}

// set and get calibrating flag, stops arming if true
void rcs_set_calibrating(bool b) { rcs.gcs_is_calibrating = b; }
bool rcs_get_calibrating() { return rcs.gcs_is_calibrating; }


/// ops
// this function is implemented in the child class in the vehicle
// code
rc_channel_t rcs_get_channel(const uint8_t chan)
{
    if (rcs.ops != NULL && rcs.ops->channel != NULL) {
        return rcs.ops->channel(&rcs, chan);
    }

    return NULL;
}

void rcs_read_mode_switch()
{
    if (rcs.ops != NULL && rcs.ops->read_mode_switch != NULL) {
        rcs.ops->read_mode_switch(&rcs);
        return;
    }

    rcs_ReadModeSwitch();
}

// has_valid_input should be pure-virtual when Plane is converted
bool rcs_has_valid_input()
{
    if (rcs.ops != NULL && rcs.ops->has_valid_input != NULL) {
        return rcs.ops->has_valid_input(&rcs);
    }

    return false;
}

rc_channel_t rcs_get_arming_channel(void)
{
    if (rcs.ops != NULL && rcs.ops->get_arming_channel != NULL) {
        return rcs.ops->get_arming_channel(&rcs);
    }

    return NULL;
}

bool rcs_arming_check_throttle()
{
    if (rcs.ops != NULL && rcs.ops->arming_check_throttle != NULL) {
        return rcs.ops->arming_check_throttle(&rcs);
    }

    return rcs._options & (uint32_t)(RCS_ARMING_CHECK_THROTTLE);
}

// flight_mode_channel_number must be overridden in vehicle specific code
int8_t rcs_flight_mode_channel_number()
{
    RT_ASSERT(rcs.ops->flight_mode_channel_number != RT_NULL);

    return rcs.ops->flight_mode_channel_number(&rcs);
}

bool rcs_ArmingCheckThrottle()
{
    return rcs._options & (uint32_t)(RCS_ARMING_CHECK_THROTTLE);
}

void rcs_ReadModeSwitch()
{
    if (!rcs_has_valid_input()) {
        // exit immediately when no RC input
        return;
    }
    rc_channel_t c = rcs_flight_mode_channel();
    if (c == NULL) {
        return;
    }
    rc_read_mode_switch(c);
}

static void rcs_assign_param()
{
    param_link_variable(PARAM_ID(RC, RC_OVERRIDE_TIME), &rcs._override_timeout);
    param_link_variable(PARAM_ID(RC, RC_OPTIONS), &rcs._options);
    param_link_variable(PARAM_ID(RC, RC_PROTOCOLS), &rcs._protocols);
    param_link_variable(PARAM_ID(RC, RC_FS_TIMEOUT), &rcs._fs_timeout);

    RC_PARAM_LINK_VARIABLE(rcs_chan(0), 1);
    RC_PARAM_LINK_VARIABLE(rcs_chan(1), 2);
    RC_PARAM_LINK_VARIABLE(rcs_chan(2), 3);
    RC_PARAM_LINK_VARIABLE(rcs_chan(3), 4);
    RC_PARAM_LINK_VARIABLE(rcs_chan(4), 5);
    RC_PARAM_LINK_VARIABLE(rcs_chan(5), 6);
    RC_PARAM_LINK_VARIABLE(rcs_chan(6), 7);
    RC_PARAM_LINK_VARIABLE(rcs_chan(7), 8);
    RC_PARAM_LINK_VARIABLE(rcs_chan(8), 9);
    RC_PARAM_LINK_VARIABLE(rcs_chan(9), 10);
    RC_PARAM_LINK_VARIABLE(rcs_chan(10), 11);
    RC_PARAM_LINK_VARIABLE(rcs_chan(11), 12);
    RC_PARAM_LINK_VARIABLE(rcs_chan(12), 13);
    RC_PARAM_LINK_VARIABLE(rcs_chan(13), 14);
    RC_PARAM_LINK_VARIABLE(rcs_chan(14), 15);
    RC_PARAM_LINK_VARIABLE(rcs_chan(15), 16);
#if NUM_RC_CHANNELS > 16
    RC_PARAM_LINK_VARIABLE(rcs_chan(16), 17);
    RC_PARAM_LINK_VARIABLE(rcs_chan(17), 18);
#endif
}

static void rcs_publish_vehicle_rc()
{
    uint8_t channel_count = 0;

    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        if (rc_has_override(rcs_chan(i)) && !rcs_ignore_receiver()) {
            channel_count ++;
        }
        actuator_rc.channels[i]  = rcs.channels[i]->radio_in;
    }

    if (rcs_has_active_overrides() && !rcs_ignore_receiver()) {
        actuator_rc.timestamp_us  = (uint64_t)rcs.last_update_ms * 1000;
        actuator_rc.rssi          = 50;
        actuator_rc.rc_protocol   = "gcs";
        actuator_rc.channel_count = channel_count;
    } else if (rcs_has_had_rc_receiver() && !rcs_ignore_receiver()) {
        actuator_rc.timestamp_us = (uint64_t)rcs.last_update_ms * 1000;
        actuator_rc.rssi         = rc_hal._rssi;
        actuator_rc.rc_protocol  = rc_hal.last_protocol;
        actuator_rc.channel_count = MIN(NUM_RC_CHANNELS, rc_hal._num_channels);
    }

    actuator_rc.override_mask = rcs_get_override_mask();

    itc_publish(ITC_ID(vehicle_rc), &actuator_rc);
}

/*------------------------------------test------------------------------------*/


